#include <stdio.h>
#include <hi_io.h>
#include <hi_pwm.h>
#include <stdlib.h>
#include <memory.h>
#include <hi_gpio.h>
#include <hi_task.h>
#include <hi_time.h>
#include <hi_watchdog.h>
#include <hi_types_base.h>
#include <hi_early_debug.h>
#include "hi_sem.h"
#include "hi_timer.h"
#include "hi_task.h"
#include <hisignalling_protocol.h>
#include <ultrasonic.h>
#include <ultrasonic.h>
#include <hi_time.h>
#include <hi_adc.h>
#include <hi_uart.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include "MPU6050.h"
#include "wifiiot_errno.h"
#include "wifiiot_gpio.h"
#include "wifiiot_gpio_ex.h"
#include "wifiiot_i2c.h"
#include "wifiiot_i2c_ex.h"

#include <math.h>
#include <hi_early_debug.h>
#include <hi_task.h>
#include <hi_time.h>
#include <hi_adc.h>
#include <hi_stdlib.h>
#include <hi_watchdog.h>
#include <hi_i2c.h>
#include <hi_io.h>
#include <hi_gpio.h>
#include <math.h>

#include <hi_time.h>
#include <hi_adc.h>
#include <hi_uart.h>
#define __INTERFACE_ROM_HI_TIMER_H__
#define PWM_FREQ    (60000)
#define __HI_ISR_H__
#include "hi_types.h"
#define HI_EXC_FLAG_NO_FLOAT                0x10000000
#define HI_EXC_FLAG_FAULTADDR_VALID         0x01
#define HI_EXC_FLAG_IN_HWI    0x02
#define HI_SPEED    3 //foward spee
int dis;
float Z;
hi_bool  alernater = HI_FALSE;
hi_bool  turnback_flag = HI_FALSE;
hi_bool  car_flag = HI_FALSE;//stop the car
float Target_A= 3,Target_B = 3;
extern hi_float car_get_distance(hi_void);
void turnbackleft(){ 
 //Target_B=0;Target_A=0;car_flag = HI_FALSE;
 turnback_flag = HI_TRUE;
 hi_sleep(1500);
 if(turnback_flag==HI_TRUE)//多余
{ 
  
    while (1)
    {engine_turn_left();//turnback step:1
    change3();
 if (abs(Z)>14)break;
    }
  engine_turn_right();//step:2
   change_2();
hi_sleep(1800);
  engine_turn_left();//step3
    change3(); 
    hi_sleep(1400);if(turnback_flag==HI_TRUE){
Target_A=0;
 Target_B=0; 
 
   turnback_flag = HI_FALSE;} turnback_flag = HI_FALSE;
     }hi_sleep(1000);alernater=HI_TRUE;
}
void turnbackright(){ 
 turnback_flag = HI_TRUE;
 hi_sleep(1500);
 if(turnback_flag==HI_TRUE)//多余
{     while (1)
    {engine_turn_right();
    change3();
 if (abs(Z)>14)break;
    }
  engine_turn_left();
   change_2();
   hi_sleep(1800);
     engine_turn_right();
    change3();
    hi_sleep(1500);if(turnback_flag==HI_TRUE){
Target_A=0;
 Target_B=0; 
 
   turnback_flag = HI_FALSE;} turnback_flag = HI_FALSE;
     }hi_sleep(1000);alernater=HI_FALSE;
}

void turnback(){
  
  
  if (alernater==HI_FALSE)turnbackleft();
  else turnbackright(); 
  
}
//纠偏
void correct(){
if (dis<15)//避障
{
 car_flag = HI_FALSE;
  Target_A=0;
 Target_B=0; 
}else if(alernater==HI_TRUE )correct_left();//左纠正
  else correct_right();//右纠正
  
  //else change0();

}
void correct_left(){
   engine_turn_right();
   change_3();
   hi_sleep(300);
  }
void correct_right(){
   engine_turn_left();
   change_3();
    hi_sleep(300);
  }
void change_2(){ 
car_flag = HI_TRUE;
  Target_A=-2;
 Target_B=-2;  
} 
void change0(){ 
car_flag = HI_FALSE;
  Target_A=0;
 Target_B=0; 
}
void change2(){ 
turnback_flag = HI_FALSE;
car_flag = HI_TRUE;
  Target_A=2;
 Target_B=2; 
}
void change3(){ 
printf("dis%d",dis);//避障
if (dis<15)
{
 car_flag = HI_FALSE;
 
}else
{
  turnback_flag = HI_FALSE;
car_flag = HI_TRUE;
  Target_A=HI_SPEED;
 Target_B=HI_SPEED;
}



 }
void change_3(){ 
car_flag = HI_TRUE;
  Target_A=-3;
 Target_B=-3; 
}
void change_4(){ 
car_flag = HI_TRUE;
  Target_A=-4;
 Target_B=-4; 
}
float Velocity_KP =20, Velocity_KI =0.0035,Velocity_KD=0;

 void Set_Pwm(); 
hi_void direction_L(hi_void);
hi_void direction_R(hi_void);
hi_void ENCODER_LL(hi_void)
{ 
    (hi_void)hi_gpio_init();
    hi_io_set_pull(HI_IO_NAME_GPIO_5, HI_IO_PULL_UP);  
    hi_io_set_pull(HI_IO_NAME_GPIO_7, HI_IO_PULL_DOWN);
    hi_gpio_set_dir(HI_GPIO_IDX_5,HI_GPIO_DIR_IN);
    hi_gpio_set_dir(HI_GPIO_IDX_7,HI_GPIO_DIR_IN);
    hi_gpio_register_isr_function(HI_GPIO_IDX_5, HI_INT_TYPE_EDGE,HI_GPIO_EDGE_FALL_LEVEL_LOW,direction_L,HI_NULL);
    }      
hi_void ENCODER_RR(hi_void)
{  
    //两个霍尔编码器 
    (hi_void)hi_gpio_init();
    hi_io_set_pull(HI_IO_NAME_GPIO_9, HI_IO_PULL_UP);  
    hi_io_set_pull(HI_IO_NAME_GPIO_10, HI_IO_PULL_DOWN);
    hi_gpio_set_dir(HI_GPIO_IDX_9,HI_GPIO_DIR_IN);
    hi_gpio_set_dir(HI_GPIO_IDX_10,HI_GPIO_DIR_IN);
    hi_gpio_register_isr_function(HI_GPIO_IDX_9, HI_INT_TYPE_EDGE,HI_GPIO_EDGE_FALL_LEVEL_LOW,direction_R,HI_NULL);//检测下降沿触发GPIO9
    }    

    hi_u32 (ENCODER1);
hi_u32 (ENCODER2);
      hi_void ENCODER_R_task(hi_void)
{
    hi_u32 ret = 0;
    hi_task_attr attr = {0};
    
    
    attr.stack_size = 1024*4;
    attr.task_prio = 28;
    attr.task_name = (hi_char*)"car_demo";
    
    ret = hi_task_create(&ENCODER1, &attr, ENCODER_RR, HI_NULL);
    if (ret != HI_ERR_SUCCESS) {
        printf("Falied to create car control demo task!\n");
    }}
      hi_void ENCODER_L_task(hi_void)
{
    hi_u32 ret = 0;
    hi_task_attr attr = {0};
    
    
    attr.stack_size = 1024*4;
    attr.task_prio = 28;
    attr.task_name = (hi_char*)"car_demo";
    
    ret = hi_task_create(&ENCODER2, &attr, ENCODER_LL, HI_NULL);
    if (ret != HI_ERR_SUCCESS) {
        printf("Falied to create car control demo task!\n");
    }}
float Velocity_L=0;
float Velocity_R=0;
  hi_void direction_L(hi_void)//左编码器方向
    { 
    hi_gpio_value value1;
    hi_gpio_get_input_val(HI_GPIO_IDX_7,&value1);
    if (value1 == HI_GPIO_VALUE0)
     {
     Velocity_L++;
     }
  else if(value1 == HI_GPIO_VALUE1) 
   {
    Velocity_L--;
   }
  } 
   hi_void direction_R(hi_void)//oyu编码器方向
    { 
    hi_gpio_value value_R;
    hi_gpio_get_input_val(HI_GPIO_IDX_10,&value_R);
    if (value_R == HI_GPIO_VALUE0)
     {
     Velocity_R++;
     }
  else if(value_R == HI_GPIO_VALUE1) 
   {
    Velocity_R--;
   }
  } 
 int Incremental_PI_A(int Encoder_L,int Target_A)
  { 
    float Bias_L;
 static float Pwm_L=0,Last_bias_L=0;
  Bias_L=Target_A-Encoder_L;                              
  Pwm_L+=Velocity_KP*(Bias_L-Last_bias_L)+Velocity_KI*Bias_L;                              
   Last_bias_L = Bias_L;                        
   return Pwm_L;     
   }             //增量输出
  int Incremental_PI_B(int Encoder_R,int Target_B)
  { 
    float Bias_R;
 static float Pwm_R=0,Last_bias_R=0;
  Bias_R=Target_B-Encoder_R;                                //计算偏差
  Pwm_R+=Velocity_KP*(Bias_R-Last_bias_R)+Velocity_KI*Bias_R; 
   Last_bias_R = Bias_R;    //保存上一次偏差                                                 
   return Pwm_R;     
   }             //增量输出

float Motora; float Motorb;
  void control_L() 
  {
   float Velocity_Left = Velocity_L;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度
  Motora = Incremental_PI_A(Velocity_Left,Target_A); //===速度PI控制器
  }
 void control_R() 
  {
   float Velocity_Right = Velocity_R;    Velocity_R = 0; 
  Motorb = Incremental_PI_B(Velocity_Right,Target_B); 
  }
 void example_entry_timer_L(void)
{
 hi_u32 timer_id1;
 hi_timer_create(&timer_id1);
 hi_sleep(120);
 hi_timer_start(timer_id1, HI_TIMER_TYPE_PERIOD, 10, control_L, 0);//启动周期性软件定时器
}
 void example_entry_timer_R(void)
{
 hi_u32 timer_id2;
 hi_timer_create(&timer_id2);
 hi_sleep(120);
 hi_timer_start(timer_id2, HI_TIMER_TYPE_PERIOD, 10, control_R, 0);
}

void  mpu6050(){
     Z = GetData(0x47) / 50;
        
        printf("yaw : %.02f \n",Z);
    
}

void MPU(void)//get yaw
{InitMPU6050();	//初始化MPU6050
	hi_sleep(150);
 hi_u32 mpu;
 hi_timer_create(&mpu);
 hi_sleep(120);
 hi_timer_start(mpu, 1, 20, mpu6050, 0);
}
void diantance(){
 
 
  dis=car_get_distance();
  printf("dis%d",dis);
}
void get_distance(void)//测距
{
 hi_u32 dis;
 hi_timer_create(&dis);
 hi_sleep(120);
 hi_timer_start(dis, HI_TIMER_TYPE_PERIOD, 200, diantance, 0);
}



void Set_Pwm_L();
void Set_Pwm_R();
hi_u32 (task_id_R);
hi_u32 (task_id_L);
hi_void car_task_L(hi_void)
{
    hi_u32 ret = 0;
    hi_task_attr attr = {0};
    
    hi_task_lock();
    attr.stack_size = 1024*4;
    attr.task_prio = 25;
    attr.task_name = (hi_char*)"car_demo";
    
    ret = hi_task_create(&task_id_L, &attr, Set_Pwm_L, HI_NULL);
    if (ret != HI_ERR_SUCCESS) {
        printf("Falied to create car control demo task!\n");
    }
    hi_task_unlock();
}
hi_void car_task_R(hi_void)
{
    hi_u32 ret = 0;
    hi_task_attr attr = {0};
    
    
    attr.stack_size = 1024*4;
    attr.task_prio = 28;
    attr.task_name = (hi_char*)"car_demo";
    
    ret = hi_task_create(&task_id_R, &attr, Set_Pwm_R, HI_NULL);
    if (ret != HI_ERR_SUCCESS) {
        printf("Falied to create car control demo task!\n");
    }}

int numb = 1000;
hi_void pwm_control(hi_io_name id, hi_u8 val, hi_pwm_port port, hi_u16 duty)
{
    hi_io_set_func(id, val);
    hi_pwm_init(port);
    hi_pwm_set_clock(PWM_CLK_160M);
    hi_pwm_start(port, duty, PWM_FREQ);
}
 hi_void gpio_control(hi_io_name gpio, hi_gpio_idx id, hi_gpio_dir  dir,  hi_gpio_value gpio_val, hi_u8 val)
{
    hi_io_set_func(gpio, val);
    hi_gpio_set_dir(id, dir);
    hi_gpio_set_ouput_val(id, gpio_val);
}
 extern hi_bool  g_uart_receive_flag ;
    void Set_Pwm_R(){  
       int speed_R,c,out;
    while(1){      // gy521_mpu6050();
      //z = GetData(0x47) / 50;
     // printf("yaw:%d\n\r",z);
 
      hi_sleep(20);

      if ((hi_max(Motorb,Motora) >= 0)&&(car_flag == HI_TRUE)) {
      speed_R=hi_max(Motorb,Motora);
    out=speed_R*numb;
      if(out > 60000)out=45000;
      if(out <= 35000)out=35000; 
                 //限幅
      gpio_control(HI_IO_NAME_GPIO_2, HI_GPIO_IDX_2, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_2_GPIO);
      gpio_control(HI_IO_NAME_GPIO_3, HI_GPIO_IDX_3, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_3_GPIO);
      pwm_control(HI_IO_NAME_GPIO_8,HI_IO_FUNC_GPIO_8_PWM1_OUT,HI_PWM_PORT_PWM1,out); 
    hi_sleep(20);//signalfafaf
     // printf("MotorB:%d\n\r",speed_R);//小车正转
      }  
      else if ((hi_max(Motorb,Motora) < 0)&&(car_flag == HI_TRUE))
      { 
        c = abs(hi_min(Motorb,Motora));  
        out=c*numb;
      if(out > 60000)out=45000;
      if(out <= 35000)out=35000;
    
      gpio_control(HI_IO_NAME_GPIO_2, HI_GPIO_IDX_2, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_2_GPIO);
      gpio_control(HI_IO_NAME_GPIO_3, HI_GPIO_IDX_3, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_3_GPIO);
      pwm_control(HI_IO_NAME_GPIO_8,HI_IO_FUNC_GPIO_8_PWM1_OUT,HI_PWM_PORT_PWM1,out); hi_sleep(20);
      
      // printf("MotoraB:%f\n\r",c);//小车反转
     }else{
        gpio_control(HI_IO_NAME_GPIO_2, HI_GPIO_IDX_2, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_2_GPIO);
      gpio_control(HI_IO_NAME_GPIO_3, HI_GPIO_IDX_3, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_3_GPIO);
      pwm_control(HI_IO_NAME_GPIO_8,HI_IO_FUNC_GPIO_8_PWM1_OUT,HI_PWM_PORT_PWM1,0);
     }
     hi_watchdog_feed();
        }
 }    


//舵机控制
hi_void set_angle(hi_s32 duty)
{ 
  
    gpio_control(HI_IO_NAME_GPIO_6, HI_GPIO_IDX_6, HI_GPIO_DIR_OUT, HI_GPIO_VALUE1, HI_IO_FUNC_GPIO_6_GPIO);
    hi_udelay(duty);
    gpio_control(HI_IO_NAME_GPIO_6, HI_GPIO_IDX_6, HI_GPIO_DIR_OUT, HI_GPIO_VALUE0, HI_IO_FUNC_GPIO_6_GPIO);
    hi_udelay(20000-duty);
     
}
hi_void engine_turn_left(hi_void)
{
    for (int i = 0; i <10; i++) 
    {
        set_angle(1100);
        hi_sleep(10);
        
    }
}
hi_void regress_middle(hi_void)
{
    for (int i = 0; i <10; i++) 
    {
        set_angle(1500);
        hi_sleep(10);
    }
}
hi_void engine_turn_right(hi_void)
{
    for (int i = 0; i <10; i++) 
    {
        set_angle(1900);
        hi_sleep(10);
    }
}
hi_void engine_turn_small_right(hi_void)
{
    for (int i = 0; i <10; i++) 
    {
        set_angle(1650);
        hi_sleep(10);
    }
}
hi_void engine_turn_small_left(hi_void)
{
    for (int i = 0; i <10; i++) 
    {
        set_angle(1350);
        hi_sleep(10);
    }
}

/*Measure distance from obstacles ahead*/
